﻿using ACS.SPiiPlusNET;
using Company.Core.Helpers;
using System.ComponentModel.Design;

namespace Company.Hardware.Acs
{

    public class AcsController : IAcsController
    {
        #region 委托
        /// <summary>
        /// Acs连接事件回调
        /// </summary>
        public delegate void AcsConnectingCallback();
        /// <summary>
        /// Acs断开连接事件回调
        /// </summary>
        public delegate void AcsDisconnectingCallback();
        /// <summary>
        /// Acs连接后事件回调
        /// </summary>
        public delegate void AcsConnectedCallback();
        /// <summary>
        /// Acs断开连接后事件回调
        /// </summary>
        public delegate void AcsDisconnectedCallback();
        /// <summary>
        /// Acs异常事件回调
        /// </summary>
        /// <param name="ex"></param>
        public delegate void AcsExceptionCallback(object ex);
        #endregion
        #region 事件
        public event AcsConnectingCallback AcsConnecting;
        public event AcsDisconnectingCallback AcsDisconnecting;
        public event AcsConnectedCallback AcsConnected;
        public event AcsDisconnectedCallback AcsDisconnected;
        public event AcsExceptionCallback AcsExceptionEvent;
        #endregion
        #region 字段
        private bool _IsConnected = false;
        private string _IpAddress = "10.0.0.100";
        private int _Port = 701;
        private double _AxesCount = 0;
        private double _BuffersCount = 0;
        #endregion
        #region 属性
        public Api AcsInstance { get; private set; }
        public string IpAddress 
        {
            get => _IpAddress;
            set 
            { 
                _IpAddress = value;
            }
        }
        public int Port 
        { 
            get => _Port;
            set 
            {
                _Port = value;
            }
        }
        public double AxesCount 
        {
            get=> _AxesCount;
            private set 
            {
                _AxesCount = value;
            }
        }
        public double BuffersCount 
        {
            get => _BuffersCount;
            private set 
            {
                _BuffersCount = value;
            } 
        }
        public string LastExceptionMessage { get; set; }
        /// <summary>
        /// Acs是否已经连接
        /// </summary>
        public bool IsConnected
        {
            get => _IsConnected;
            private set 
            {
                _IsConnected = value;
            }
        }
        #endregion
        #region 构造函数
        public AcsController(Api api)
        {
            AcsInstance = api;
        }
        #endregion

        #region AcsApi
        /// <summary>
        /// 连接到EthernetTCP
        /// </summary>
        public void ConnectToEthernetTCP()
        {
            AcsConnecting?.Invoke();
            try
            {
                AcsInstance.OpenCommEthernetTCP(IpAddress, Port);
                IsConnected = AcsInstance.IsConnected;
                AcsInstance.RegisterEmergencyStop();
                AcsConnected?.Invoke();

                //获取Acs控制器信息
                AxesCount = AcsInstance.GetAxesCount();//轴数量
                BuffersCount = AcsInstance.GetBuffersCount();//Buffer数量
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 连接到EthernetTCP
        /// </summary>
        /// <param name="address"></param>
        /// <param name="port"></param>
        public void ConnectToEthernetTCP(string address, int port)
        {
            AcsConnecting?.Invoke();
            IpAddress = address;
            Port = port;
            try
            {
                AcsInstance.OpenCommEthernetTCP(IpAddress, Port);
                //注册急停图标，即使ACS上位机控制软件没有打开，也可以显示急停图标组件
                //AcsInstance.RegisterEmergencyStop();
                IsConnected = AcsInstance.IsConnected;
                //获取Acs控制器信息
                AxesCount = AcsInstance.GetAxesCount();//轴数量
                BuffersCount = AcsInstance.GetBuffersCount();//Buffer数量
                AcsConnected?.Invoke();
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 连接到虚拟仿真
        /// </summary>
        /// <param name="address"></param>
        /// <param name="port"></param>
        public void ConnectToSimulator()
        {
            AcsConnecting?.Invoke();
            try
            {
                AcsInstance.OpenCommSimulator();
                //AcsInstance.RegisterEmergencyStop();
                AcsConnected?.Invoke();
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 写入变量
        /// </summary>
        /// <param name="value">值</param>
        /// <param name="variable">变量名</param>
        public void WriteVariable(object value, string variable)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.WriteVariable(value, variable);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 写入变量
        /// </summary>
        /// <param name="value">值</param>
        /// <param name="variable">变量名</param>
        public void WriteVariable(object value, string variable, ProgramBuffer programBuffer)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.WriteVariable(value, variable, programBuffer);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 发送命令
        /// </summary>
        /// <param name="command"></param>
        /// <returns></returns>
        public string SendCommand(string command)
        {
            if (!IsConnected) return "Controller is not connected";
            try
            {
                return AcsInstance.Transaction(command);
            }
            catch (ACSException ex)
            {
                var errorMessage = $"Command:{command}" + $"Message:{ex.Message}";
                //LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(command + Environment.NewLine + ex.Message);
                return errorMessage;
            }
        }
        /// <summary>
        /// 断开控制器连接
        /// </summary>
        public void Disconnect()
        {
            //AcsInstance.UnregisterEmergencyStop();
            AcsDisconnecting?.Invoke();
            if (IsConnected)
            {
                AcsInstance.CloseComm();
                IsConnected = AcsInstance.IsConnected;
                AcsDisconnected?.Invoke();
            }
        }
        /// <summary>
        /// 轴使能
        /// </summary>
        /// <param name="axis"></param>
        public void Enable(Axis axis)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.Enable(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 轴断使能
        /// </summary>
        /// <param name="axis"></param>
        public void Disable(Axis axis)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.Disable(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 使能Fault
        /// </summary>
        public void EnableFault(Axis axis, SafetyControlMasks safetyControlMasks)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.EnableFault(axis, safetyControlMasks);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 禁用Fault
        /// </summary>
        public void DisableFault(Axis axis, SafetyControlMasks safetyControlMasks)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.DisableFault(axis, safetyControlMasks);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 暂停
        /// </summary>
        /// <param name="axis">目标轴对象</param>
        public void Halt(Axis axis)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.Halt(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// Jog运动
        /// </summary>
        /// <param name="motionFlags">运动标志</param>
        /// <param name="axis">目标轴对象（ID）</param>
        /// <param name="velocity">速度</param>
        public void Jog(MotionFlags motionFlags, Axis axis, double velocity)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.Jog(MotionFlags.ACSC_AMF_VELOCITY, axis, velocity);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// PTP运动
        /// </summary>
        /// <param name="motionFlags"></param>
        /// <param name="axis"></param>
        /// <param name="position">目标位置</param>
        public void ToPoint(MotionFlags motionFlags, Axis axis, double position)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.ToPoint(motionFlags, axis, position);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 目标轴是否回参完成
        /// </summary>
        /// <param name="axis">目标轴</param>
        public bool ReadAxisIsReferenced(Axis axis)
        {
            if (!IsConnected) return false;
            bool isReferenced = false;
            try
            {
                int mFlag = (int)AcsInstance.ReadVariable("MFLAGS", ProgramBuffer.ACSC_NONE, (int)axis, (int)axis);
                // MFLAGS(0).#HOME,回参完成标志为数组MFLAGS(0)第三位
                isReferenced = IntOfBitHelper.GetBitValue(mFlag, 3);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return isReferenced;
        }
        /// <summary>
        /// 目标轴是否回参完成
        /// </summary>
        /// <param name="axis">目标轴</param>
        public async Task<bool> ReadAxisIsReferencedAsync(Axis axis,int timeout)
        {
            if (!IsConnected) return false;
            bool isReferenced = false;
            try
            {
                await Task.Run(() => 
                {
                    ACSC_WAITBLOCK wait = AcsInstance.ReadVariableAsync("MFLAGS", ProgramBuffer.ACSC_NONE, (int)axis, (int)axis);
                    int mFlag = (int)AcsInstance.GetResult(wait, timeout);
                    // MFLAGS(0).#HOME,回参完成标志为数组MFLAGS(0)第三位
                    isReferenced = IntOfBitHelper.GetBitValue(mFlag, 3);
                });
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return isReferenced;
        }
        /// <summary>
        /// 获取电机状态
        /// </summary>
        /// <param name="axis"></param>
        /// <returns></returns>
        public MotorStates GetMotorStates(Axis axis)
        {
            MotorStates motorStates = MotorStates.ACSC_NONE;
            if (IsConnected)
            {
                try
                {
                    motorStates = AcsInstance.GetMotorState(axis);
                }
                catch (ACSException ex)
                {
                    LastExceptionMessage = ex.Message;
                    AcsExceptionEvent?.Invoke(ex);
                }
            }
            return motorStates;
        }
        /// <summary>
        /// 停止所有轴运动
        /// </summary>
        public void KillAllAxes()
        {
            try
            {
                AcsInstance.KillAll();
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 获取电机错误码
        /// </summary>
        /// <param name="axis"></param>
        /// <returns>错误码</returns>
        public int GetMotorError(Axis axis)
        {
            var error = 0;
            if (IsConnected)
            {
                try
                {
                    error = AcsInstance.GetMotorError(axis);
                }
                catch (ACSException ex)
                {
                    LastExceptionMessage = ex.Message;
                    AcsExceptionEvent?.Invoke(ex);
                }
            }
            return error;
        }
        /// <summary>
        /// 获取错误码详细信息
        /// </summary>
        /// <param name="errorCode"></param>
        /// <returns></returns>
        public string GetErrorString(int errorCode)
        {
            if (!IsConnected) return "";
            if (errorCode == 0) return "";
            string errorMessage = "";
            try
            {
                errorMessage = AcsInstance.GetErrorString(errorCode);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return errorMessage;
        }
        /// <summary>
        /// 获取指定MotorError
        /// </summary>
        /// <returns></returns>
        public bool GetFault(Axis axis)
        {
            if (!IsConnected) return false;
            SafetyControlMasks safetyControlMasks = SafetyControlMasks.ACSC_NONE;
            try
            {
                safetyControlMasks = AcsInstance.GetFault(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            if (safetyControlMasks == SafetyControlMasks.ACSC_NONE
                || safetyControlMasks == SafetyControlMasks.ACSC_ALL
                || safetyControlMasks == SafetyControlMasks.ACSC_SAFETY_HSSINC
                || safetyControlMasks == SafetyControlMasks.ACSC_SAFETY_EXTNT
                || safetyControlMasks == SafetyControlMasks.ACSC_SAFETY_TEMP
                || safetyControlMasks == SafetyControlMasks.ACSC_SAFETY_FAILURE)
                return false;
            else
                return true;
        }
        /// <summary>
        /// 重启控制器
        /// </summary>
        /// <param name="timeOut"></param>
        public void ControllerReboot(int timeOut)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.ControllerReboot(timeOut);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 重启控制器并恢复出厂设置
        /// </summary>
        /// <param name="timeOut"></param>
        public void ControllerRebootDefualt(int timeOut)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.ControllerFactoryDefault(timeOut);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 获取当前速度
        /// </summary>
        /// <param name="axis"></param>
        /// <returns></returns>
        public double GetCurrentVelocity(Axis axis)
        {
            double currentVelocity = 0;
            try
            {
                currentVelocity = (double)AcsInstance.ReadVariable("FVEL", ProgramBuffer.ACSC_NONE, (int)axis, (int)axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return currentVelocity;
        }
        /// <summary>
        /// 获取急停减速度
        /// </summary>
        /// <param name="axis"></param>
        /// <returns></returns>
        public double GetKillDeceleration(Axis axis)
        {
            double currentKillDeceleration = 0;
            try
            {
                currentKillDeceleration = (double)AcsInstance.GetKillDeceleration(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return currentKillDeceleration;
        }
        /// <summary>
        /// 获取当前位置误差
        /// </summary>
        /// <param name="axis"></param>
        /// <returns></returns>
        public double GetCurrentPositionError(Axis axis)
        {
            double currentPositionError = 0;
            try
            {
                currentPositionError = (double)AcsInstance.ReadVariable("PE", ProgramBuffer.ACSC_NONE, (int)axis, (int)axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return currentPositionError;
        }
        /// <summary>
        /// 获取当前位置反馈
        /// </summary>
        /// <param name="axis"></param>
        /// <returns></returns>
        public double GetCurrentFeedbackPosition(Axis axis)
        {
            double currentFeedbackPosition = 0;
            try
            {
                currentFeedbackPosition = AcsInstance.GetFPosition(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return currentFeedbackPosition;
        }
        /// <summary>
        /// 获取当前回参位置
        /// </summary>
        /// <param name="axis"></param>
        /// <returns></returns>
        public double GetCurrentReferencePosition(Axis axis)
        {
            double currentReferencePosition = 0;
            try
            {
                currentReferencePosition = AcsInstance.GetRPosition(axis);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return currentReferencePosition;
        }
        /// <summary>
        /// 读变量
        /// </summary>
        /// <param name="variable"></param>
        /// <param name="programBuffer"></param>
        /// <param name="from1"></param>
        /// <param name="to1"></param>
        /// <param name="from2"></param>
        /// <param name="to2"></param>
        /// <returns></returns>
        public object ReadVariable(string variable, ProgramBuffer programBuffer = ProgramBuffer.ACSC_NONE, int from1 = -1, int to1 = -1, int from2 = -1, int to2 = -1)
        {
            if (!IsConnected) return null;
            try
            {
                return AcsInstance.ReadVariable(variable, programBuffer, from1, to1, from2, to2);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return null;
        }
        /// <summary>
        /// 读变量Int32
        /// </summary>
        /// <param name="variable"></param>
        /// <param name="programBuffer"></param>
        /// <param name="from1"></param>
        /// <param name="to1"></param>
        /// <param name="from2"></param>
        /// <param name="to2"></param>
        /// <returns></returns>
        public bool ReadIntVariableByBit(string variable, byte index, ProgramBuffer programBuffer = ProgramBuffer.ACSC_NONE, int from1 = -1, int to1 = -1, int from2 = -1, int to2 = -1)
        {
            try
            {
                int returnValue = Convert.ToInt32(AcsInstance.ReadVariable(variable, programBuffer, from1, to1, from2, to2));
                return IntOfBitHelper.GetBitValue<Int32>(returnValue, index);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            catch (ArgumentOutOfRangeException ex)
            {
                LastExceptionMessage = ex.Message;
            }
            catch (ArgumentException ex)
            {
                LastExceptionMessage = ex.Message;
            }
            return false;
        }
        /// <summary>
        /// 写变量
        /// </summary>
        /// <param name="value">值</param>
        /// <param name="variable">变量名</param>
        /// <param name="programBuffer"></param>
        /// <param name="from1"></param>
        /// <param name="to1"></param>
        /// <param name="from2"></param>
        /// <param name="to2"></param>
        public void WriteVariable(object value, string variable, ProgramBuffer programBuffer = ProgramBuffer.ACSC_NONE, int from1 = -1, int to1 = -1, int from2 = -1, int to2 = -1)
        {
            try
            {
                AcsInstance.WriteVariable(value, variable, programBuffer, from1, to1, from2, to2);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 保存到Flash缓冲区
        /// </summary>
        public void SaveToFlash()
        {
            Axis[] parameters = new Axis[2];
            ProgramBuffer[] buffers = new ProgramBuffer[2];
            ServoProcessor[] sPPrograms = new ServoProcessor[2];
            parameters[0] = Axis.ACSC_PAR_ALL;
            parameters[1] = Axis.ACSC_NONE;
            buffers[0] = ProgramBuffer.ACSC_BUFFER_ALL;
            buffers[1] = ProgramBuffer.ACSC_NONE;
            sPPrograms[0] = ServoProcessor.ACSC_SP_ALL;
            sPPrograms[1] = ServoProcessor.ACSC_NONE;
            try
            {
                AcsInstance.ControllerSaveToFlash(parameters, buffers, sPPrograms, null);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 编译Buffer
        /// </summary>
        public void CompileBuffer(ProgramBuffer programBuffer)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.CompileBuffer(programBuffer);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 暂停Buffer
        /// </summary>
        public void SuspendBuffer(ProgramBuffer programBuffer)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.SuspendBuffer(programBuffer);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 停止Buffer
        /// </summary>
        public void StopBuffer(ProgramBuffer programBuffer)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.StopBuffer(programBuffer);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 运行Buffer
        /// </summary>
        public void RunBuffer(ProgramBuffer programBuffer)
        {
            if (!IsConnected) return;
            try
            {
                AcsInstance.RunBuffer(programBuffer, null);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 获取程序状态
        /// </summary>
        /// <param name="programBuffer"></param>
        /// <returns></returns>
        public ProgramStates GetProgramState(ProgramBuffer programBuffer)
        {
            ProgramStates state = new ProgramStates();
            try
            {
                state = AcsInstance.GetProgramState(programBuffer);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return state;
        }
        /// <summary>
        /// 获取程序错误代码
        /// </summary>
        /// <param name="programBuffer"></param>
        /// <returns></returns>
        public int GetProgramError(ProgramBuffer programBuffer)
        {
            int errorCode = 0;
            try
            {
                errorCode = AcsInstance.GetProgramError(programBuffer);
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
            return errorCode;
        }
        /// <summary>
        /// 取消注册急停组件
        /// </summary>
        public void UnregisterEmergencyStop()
        {
            try
            {
                AcsInstance.UnregisterEmergencyStop();
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        /// <summary>
        /// 注册急停组件
        /// </summary>
        public void RegisterEmergencyStop()
        {
            try
            {
                AcsInstance.RegisterEmergencyStop();
            }
            catch (ACSException ex)
            {
                LastExceptionMessage = ex.Message;
                AcsExceptionEvent?.Invoke(ex);
            }
        }
        #endregion
    }
}
